#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：bignet_sumo_dev
@File ：main-dev-tongluoshan.py
@IDE ：PyCharm
@Author ：xinyingjie
@Date ：2025/3/24 9:40
'''

from __future__ import absolute_import
from __future__ import print_function

import asyncio
import math
import queue
import json
import threading
import random
import time
from multiprocessing import Process, Queue

import traci.constants as tc
import sumolib

from kafka import KafkaProducer, KafkaConsumer
from pyproj import Proj

from config.config40 import GLOBAL_POLYGON_CENTER, MAX_SPEED, KAFKA_HOST, KAFKA_RECEIVE_TOPIC, \
    RADAR_DATA_LIST, KAFKA_SEND_TOPIC, KAFKA_E1_TOPIC, KAFKA_E1_TOPIC_SIMU, SHOW_SIMU, USE_LIBSUMO, USE_GUI
from utils import sumo_utils
from loguru import logger
from utils.tools import calculate_distance, find_vehicles_by_duration_optimized, \
    calculate_vehicle_acceleration_deceleration, get_timestamp_milliseconds, get_angle, is_in_front, \
    get_next_target_car_info
import concurrent.futures
from websocket.websocketserver1 import WebSocketServer

if USE_LIBSUMO:
    import libsumo as traci
else:
    import traci

logger.add("debug.log", format="{time} {level} {message}", level="DEBUG")


def read_trafficE1_channel(target_channel, q_r, q_s, p_id):
    # kafka_producer = KafkaProducer(bootstrap_servers=KAFKA_HOST, key_serializer=str.encode,
    #                                value_serializer=lambda x: json.dumps(x).encode('utf-8'),
    #                                compression_type='gzip')

    # ws_server = WebSocketServer()
    # ws_server.start()
    # loop = asyncio.new_event_loop()  # 创建新的事件循环
    # asyncio.set_event_loop(loop)
    # 实时接收全域E1帧数据
    kafka_consumer_E1 = KafkaConsumer(
        KAFKA_E1_TOPIC,
        bootstrap_servers=KAFKA_HOST,
        auto_offset_reset='latest',
        enable_auto_commit=False
    )
    logger.info(f"全域E1{target_channel}监听已就绪")
    RADAR_sn_set = set(RADAR_DATA_LIST.keys())
    # 存储每一辆车的多帧信息
    per_car_info_dict = {}
    sn_car_info_dict = {}
    for sn in RADAR_sn_set:
        per_car_info_dict[sn] = {}
        sn_car_info_dict[sn] = {}
    simu_json_list_before = []
    while True:
        time.sleep(0.1)
        send_time = 0
        send_count = 0
        frame_num = 0
        for message in kafka_consumer_E1:
            if message:
                # try:
                frame_num += 1
                sn_set = set()
                one_list = []
                message_value = message.value.decode('utf-8')
                data = json.loads(message_value)
                # TODO 将数据缓存添加到仿真进程内
                q_r.put(data)
                # simu_json_list = []
                # while not q_s.empty():
                #     simu_json_list = q_s.get()
                #     # print("发送端", len(simu_json_list))
                # if len(simu_json_list) > 0:
                #     simu_json_list_before = simu_json_list
                # data['obj'] = one_list + simu_json_list_before if len(
                #     simu_json_list_before) > 0 else one_list
                # logger.info(f"感知数量为{len(one_list)} 仿真数量为{len(simu_json_list_before)}")
                # kafka_producer.send(KAFKA_E1_TOPIC_SIMU, value=data,
                #                     key=str(int(time.time())))

                # future = asyncio.run_coroutine_threadsafe(ws_server.broadcast(str(data).replace("'", '"')),
                #                                           ws_server.loop)
                # future.result()  # 等待协程完成
                # error_sn_set = RADAR_sn_set - sn_set
                # # logger.error(f"故障雷达{error_sn_set}")
                # for sn in RADAR_sn_set:
                #     if sn in error_sn_set:
                #         RADAR_DATA_LIST[sn]['normal'] = False
                #     else:
                #         RADAR_DATA_LIST[sn]['normal'] = True
                # end_time = time.time()
                # if end_time - send_time > 1:
                #     logger.info(f"进程{p_id} 发送帧率 {abs(send_count - frame_num)}  加车队列长度{q_r.qsize()} 发送队列长度{q_s.qsize()}")
                #     send_time = end_time
                #     send_count = frame_num = 0

                    # self.kafka_producer.send(KAFKA_E1_TOPIC_SIMU, value=data,
                    #                          key=str(int(time.time())))
                # except Exception as e:
                #     logger.error(str(e))


class SumoMain:
    def __init__(self, sumo_net_file, sumo_cfg_file, gui, process_id):
        """
        仿真初始化
        :param sumo_net_file:  仿真路网文件路径
        :param sumo_cfg_file:  仿真配置文件路径
        :param gui: 是否开启gui
        :param process_id: 进程id
        """

        self.sumo_net_file = sumo_net_file
        self.sumo_cfg_file = sumo_cfg_file
        self.gui = gui
        self.process_id = process_id
        # 仿真路网对象
        self.sumo_net_instance = None
        # 存储卡口、雷达经纬度所在路网的路段和位置信息
        self.kakou_pos_dict = {}
        # 加载路网，启动仿真，根据雷达位置设置路由，可视化雷达位置
        self.init_sumo()
        # 根据key1 门架编号 存储 key2 车辆id 对应的 value 车辆信息 定期删除
        self.sn_car_info_dict = {}
        # 所有基站编号
        self.RADAR_sn_set = set(RADAR_DATA_LIST.keys())
        # 存储每一个门架每一辆车的多帧信息 定期删除
        self.per_car_info_dict = {}
        for sn in self.RADAR_sn_set:
            self.sn_car_info_dict[sn] = {}
            self.per_car_info_dict[sn] = {}
        # 是否开启速度控制
        self.speed_control = False
        # 正在加速列表
        self.speed_accelerate_list = []
        # 正在减速列表
        self.speed_decelerate_list = []
        # 线程池
        self.executor = concurrent.futures.ThreadPoolExecutor(max_workers=300)
        # 仿真运行的批次
        self.batch_num = 0
        # 用于统计一秒内的车辆信息
        self.global_time = time.time()
        self.global_num = 0
        # 子进程列表，用于开启和关闭
        self.son_process = []

    def init_sumo(self):
        """
        初始化sumo  加载
        :return:
        """
        # 加载路网
        self.sumo_net_instance = sumolib.net.readNet(self.sumo_net_file, withInternal=True, withLatestPrograms=True)
        # 启动仿真
        sumo_utils.start_sumo(self.sumo_cfg_file, False, gui=self.gui)
        # 设置所有车道的最大通行速度
        for id in traci.lane.getIDList():
            traci.lane.setMaxSpeed(id, MAX_SPEED)
        # 新增区域中心矩形，用于订阅100KM范围内的所有仿真车辆信息，并配置监听信息内容
        traci.polygon.add('GLOBAL_POLYGON_CENTER', GLOBAL_POLYGON_CENTER, (1, 0, 1))
        traci.polygon.subscribeContext('GLOBAL_POLYGON_CENTER',
                                       tc.CMD_GET_VEHICLE_VARIABLE, 1000000,
                                       [tc.VAR_SPEED, tc.VAR_TYPE, tc.VAR_POSITION, tc.VAR_ANGLE, ])

        # 将雷达、卡口相机的经纬度和终点位置转为路网上所在的路段id 位置距离 车道号，起始点和终点所在的路段id添加路由
        for sn, sn_info in RADAR_DATA_LIST.items():
            # 转换经纬度到路网坐标
            center_x, center_y = traci.simulation.convertGeo(sn_info['lon'], sn_info['lat'], fromGeo=True)
            EdgeID, Pos, LaneIndex = traci.simulation.convertRoad(sn_info['lon'], sn_info['lat'], True)
            next_edge_id, next_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID, Pos,
                                                                                   150)
            arrive_edge_id, arrive_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID,
                                                                                       Pos, 1850)
            # arrive_edge_id, arrive_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID,
            #                                                                            Pos, 150)

            logger.info(f"门架{sn_info['sn']} 所在路段为{EdgeID} 位置为{Pos} 下行100米后的路段为{next_edge_id} 位置为{next_pos}")
            if sn_info['sn'] not in self.kakou_pos_dict.keys():
                traci.route.add(str(sn_info['sn']), [str(next_edge_id), str(arrive_edge_id)])
                self.kakou_pos_dict[str(sn_info['sn'])] = {"EdgeID": next_edge_id, "Pos": next_pos,
                                                           'LaneIndex': LaneIndex,
                                                           'routeId': str(str(sn_info['sn']))}
            # 创建圆形区域，表示相机或雷达的位置，用于在使用gui可视化时，展示设备具体位置
            if sn_info['normal']:
                # 创建圆形区域
                self.create_circle_polygon(sn, center_x, center_y, radius=50)
            else:
                self.create_circle_polygon(sn, center_x, center_y, radius=50, color=(255, 0, 0, 255))

    def create_circle_polygon(self, vehID, center_x, center_y, radius, color=(0, 255, 0, 255), filled=True):
        """
        在指定位置创建一个近似圆形的多边形。

        :param vehID: 车辆 ID 或其他唯一标识符，用于命名多边形
        :param center_x: 圆心的 x 坐标
        :param center_y: 圆心的 y 坐标
        :param radius: 圆的半径
        :param color: 多边形的颜色 (R,G,B,A)，默认绿色不透明
        :param filled: 是否填充多边形，默认 True
        """
        # 生成圆周上的点
        num_points = 50  # 点的数量越多，圆越光滑
        points = []
        for i in range(num_points):
            angle = 2 * math.pi * i / num_points
            x = center_x + radius * math.cos(angle)
            y = center_y + radius * math.sin(angle)
            points.append((x, y))

        # 添加多边形
        polygon_id = f"circle_{vehID}"
        traci.polygon.add(polygon_id, shape=points, color=color, fill=filled, layer=0)

    def control_car(self):
        t1 = time.time()
        while not re_queue_data.empty():
            data = re_queue_data.get()
            if data:
                sn_set = set()
                e1FrameParticipant_list = data['obj']
                ts = data['ts']
                # print((int(time.time() * 1000) - ts))
                # print(e1FrameParticipant_list)
                for one in e1FrameParticipant_list:
                    # print(one)
                    sourceId = str(one['sn'])
                    one['sourceId'] = sourceId
                    sn_set.add(sourceId)
                    car_id = one['id']
                    if car_id not in self.per_car_info_dict[sourceId].keys():
                        self.per_car_info_dict[sourceId][car_id] = {}
                        self.per_car_info_dict[sourceId][car_id][ts] = one
                    else:
                        self.per_car_info_dict[sourceId][car_id][ts] = one
                continuous_vehicles, disappeared_vehicles, delete_vehicles = find_vehicles_by_duration_optimized(
                    self.per_car_info_dict, 2000, 2000, 50000)
                for sn, disappeared_vehicle_list in disappeared_vehicles.items():
                    for disappeared_vehicle in disappeared_vehicle_list:
                        dis_id = disappeared_vehicle[0]
                        if dis_id not in self.sn_car_info_dict[sn].keys():
                            self.sn_car_info_dict[sn][dis_id] = disappeared_vehicle[3]
                            car_one = disappeared_vehicle[3]
                            car_one['id'] = str(car_one['id']) + "-" + str(car_one['sn'])
                            # logger.info(f"新增车辆{car_one}")
                            disappeared_vehicle[3]['error_sn'] = list(self.RADAR_sn_set - sn_set)
                            # logger.error(f"故障雷达{self.RADAR_sn_set - sn_set}")
                            # 添加车辆
                            sn = car_one['sn']
                            # logger.info(f"添加车辆{sn}  {car_one}")
                            self.add_camera_car(car_one, sn)
                            # q_r.put(disappeared_vehicle[3])

                for c_id, s_id in delete_vehicles.items():
                    # logger.info(f"删除车辆{c_id} {per_car_info_dict[s_id].keys()}  {sn_car_info_dict[s_id].keys()}")
                    if c_id in self.per_car_info_dict[s_id].keys():
                        del self.per_car_info_dict[s_id][c_id]
                        # logger.info(f"per_car_info_dict 删除成功{c_id}")
                    if c_id in self.sn_car_info_dict[s_id].keys():
                        del self.sn_car_info_dict[s_id][c_id]
                        # logger.info(f"sn_car_info_dict 删除成功{c_id}")

            t2 = time.time()
        # logger.info(f"加车耗时{t2 - t1}")

    def add_camera_car(self, vehiclePicInfo, sn):
        try:
            car_id = vehiclePicInfo['id']

            # print("新增车辆",str(car_id),self.kakou_pos_dict[str(sn)]['routeId'],self.kakou_pos_dict[str(sn)]['Pos'],)
            set_speed = vehiclePicInfo['speed']

            traci.vehicle.add(
                vehID=str(car_id),
                routeID=self.kakou_pos_dict[str(sn)]['routeId'],
                departPos=self.kakou_pos_dict[str(sn)]['Pos'],
                typeID=int(vehiclePicInfo['type']), depart='now',
                departSpeed=str(set_speed))
            traci.vehicle.setSpeed(str(car_id),
                                   set_speed)
            # traci.vehicle.slowDown(str(car_id),int(120 / 3.6),2)

            vehiclePicInfo['id'] = str(car_id)
            vehiclePicInfo['sourceId'] = sn
            traci.vehicle.setParameter(str(car_id), "etc_info",
                                       str(vehiclePicInfo))
        except Exception as e:
            logger.error(f"加车异常{str(e)}")


    def stop_remove_car(self, global_vehicles, queue_data):
        t_start = int(time.time() * 1000)
        simu_json_list = []
        for vehicle_id, veh_info in dict(global_vehicles).items():

            pos = veh_info[tc.VAR_POSITION]

            longitude, latitude = traci.simulation.convertGeo(pos[0], pos[1], fromGeo=False)

            # sourceId = vehicle_id.split("-")[1]

            try:
                etc_info = eval(traci.vehicle.getParameter(vehicle_id, "etc_info"))
            except Exception as e:
                # logger.info("etc_info", e)
                pass
            sourceId = etc_info['sn']
            if sourceId in RADAR_DATA_LIST.keys():
                target_next_id = None
                while RADAR_DATA_LIST[sourceId]['Downstream'] is not None:
                    # print(RADAR_DATA_LIST[sourceId]['Downstream']['sn'] in  etc_info['error_sn'].copy())
                    # if RADAR_DATA_LIST[sourceId]['Downstream']['sn'] not in etc_info['error_sn']:
                    if 1:
                        target_next_id = RADAR_DATA_LIST[sourceId]['Downstream']['sn']
                        break
                    sourceId = RADAR_DATA_LIST[sourceId]['Downstream']['sn']
                if target_next_id is not None:
                    dis_next_sn = calculate_distance(longitude, latitude,
                                                     RADAR_DATA_LIST[target_next_id]['lon'],
                                                     RADAR_DATA_LIST[target_next_id]['lat'])
                    # logger.info(f"车辆{vehicle_id}距离下游门架距离为{dis_next_sn}")

                    if 100 < dis_next_sn < 150:
                        # logger.info(
                        #     f"车辆{vehicle_id}距离下游门架其他车辆距离信息======{int(vehicle_id.split('-')[0]) in list(self.per_car_info_dict[target_next_id].keys())}======================")
                        if int(vehicle_id.split('-')[0]) in list(self.per_car_info_dict[target_next_id].keys()):
                            t_info = self.per_car_info_dict[target_next_id][int(vehicle_id.split('-')[0])]
                            other_last_time = max(list(t_info.keys()))
                            other_last_info = self.per_car_info_dict[target_next_id][int(vehicle_id.split('-')[0])][
                                other_last_time]
                            dis_other = calculate_distance(longitude, latitude, other_last_info['lon'],
                                                           other_last_info['lat'])
                            logger.info(
                                f"车辆{vehicle_id}距离下游门架本车辆距离信息 时间差{(t_start - other_last_time)} 距离差{dis_other}")
                        target_car_info = get_next_target_car_info(t_start, target_next_id, longitude, latitude,
                                                                   etc_info, self.per_car_info_dict)
                        if target_car_info:
                            traci.vehicle.setParameter(str(vehicle_id), "etc_info",
                                                       str(target_car_info))
                            # etc_info[]
                            simu_is_front = is_in_front(etc_info, target_car_info)
                            logger.info(f"{vehicle_id} 在下游匹配车辆前方{simu_is_front}")
                            if simu_is_front:
                                # 减速
                                pass
                            else:
                                # 加速
                                pass
                    if dis_next_sn <= 100:
                        try:
                            traci.vehicle.remove(vehicle_id)
                            # print('删除车辆',vehicle_id)
                        except Exception as e:
                            logger.info(f"stop {e} {vehicle_id}")
                            pass

            else:
                logger.error(f"雷达 {sourceId}不在配置文件内")

            speed = veh_info[tc.VAR_SPEED]
            # logger.info(f"车辆速度为{int(speed * 3.6)},控制车速信息为{self.speed_change_control_dict}")
            vehicle_type = veh_info[tc.VAR_TYPE]
            courseAngle = veh_info[tc.VAR_ANGLE]

            laneNum = 0
            temp = {"id": etc_info['id'], "type": etc_info['type'], 'source': 4,
                    "laneNum": 0,
                    "longitude": longitude, "latitude": latitude, "speed": int(speed * 100),
                    "courseAngle": round(courseAngle, 1), 'plate': "默A00000", "flag": 0
                    }
            simu_json_list.append(temp)
        # print("仿真端", len(simu_json_list))
        queue_data.put(simu_json_list)

    def main_loop(self,stop_data):
        global re_queue_data

        # 在子进程中初始化多进程队列和资源
        queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        # Process(target=send_kafka_1, args=(queue_data, self.process_id,)).start()
        # 在子进程中初始化多进程队列和资源
        re_queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        son_process = Process(target=read_trafficE1_channel,
                              args=(KAFKA_E1_TOPIC, re_queue_data, queue_data, self.process_id), daemon=True)
        son_process.start()
        self.son_process.append(son_process)
        batch_num = 0
        low_frame_num = 0
        while True:
            # try:
            start_time = time.time()
            start_time_m = int(time.time() * 1000)

            # ================================收到门架上报数据，进行车辆处理==========================================

            global_vehicles = traci.polygon.getContextSubscriptionResults('GLOBAL_POLYGON_CENTER')
            # self.executor.submit(self.control_car)
            self.control_car()
            self.stop_remove_car(global_vehicles, queue_data)
            # self.executor.submit(self.stop_remove_car, global_vehicles.copy(), queue_data)
            self.batch_num = batch_num
            batch_num += 1
            traci.simulationStep()
            end_time = time.time()
            # print(end_time - self.global_time)
            if end_time - self.global_time > 1:
                simu_frame = abs(self.global_num - batch_num)
                logger.info(f"进程{self.process_id} 仿真帧率 {simu_frame} 仿真数量{len(global_vehicles)}")
                self.global_time = end_time
                self.global_num = batch_num

                if simu_frame < 7:
                    low_frame_num += 1
                if low_frame_num > 10 or (batch_num > 2000 and len(global_vehicles)==0):
                    logger.info(f"关闭子进程")
                    for s_p in self.son_process:
                        s_p.terminate()
                        s_p.join()
                    stop_data.put(1)
            if start_time and (end_time - start_time) < 0.1:
                time.sleep(0.1 - (end_time - start_time))
            else:
                if start_time:
                    pass
            # except Exception as e:
            #     logger.error(f"主循环异常{str(e)}")
            #     continue


def run_sumo_simulation(sumo_net_file, sumo_cfg_file, gui, process_id,stop_data):
    sumo_main = SumoMain(sumo_net_file, sumo_cfg_file, gui, process_id)
    sumo_main.main_loop(stop_data)


if __name__ == '__main__':
    # 路网文件 线上部署需改为绝对路径
    sumo_net_file = "./net/40KM.net.xml"
    # 配置文件 线上部署需改为绝对路径
    sumo_cfg_file = "./net/40KM.sumocfg"
    stop_data = Queue(maxsize=2000)  # 创建队列
    # 是否启用gui可视化界面，调试阶段启用，线上部署关闭减少资源占用
    gui = USE_GUI
    for i in range(1):
        print("开启仿真主进程")
        tess_process = Process(target=run_sumo_simulation, args=(sumo_net_file, sumo_cfg_file, gui, i,stop_data))  # 创建队列))
        tess_process.start()
        # 监听主进程状态，如果收到程序异常指令，重启仿真进程
        while True:
            stop_flag = False
            while stop_data.qsize() > 0:
                _ = stop_data.get()
                stop_flag = True
            if stop_flag:
                logger.info("关闭仿真主进程")
                tess_process.terminate()
                tess_process.join()
                logger.info("重新开启仿真主进程")
                tess_process = Process(target=run_sumo_simulation, args=(sumo_net_file, sumo_cfg_file, gui, i,stop_data))
                tess_process.start()
            time.sleep(1)